#include "ROBOT.h"

#include "MsgQueue.h"

void ROBOT::PrintI(int i)
{
	StringToAdd.Format("%d",i);
	MsgHandle->Add_Message(StringToAdd);
}

void ROBOT::PrintF(float f, int i)
{
	StringToAdd.Format("%.1f",f);
	MsgHandle->Add_Message(StringToAdd);
}

void ROBOT::PrintF2(float f2, int i)
{
	StringToAdd.Format("%.1f",f2);
	MsgHandle->Add_Message(StringToAdd);
}

void ROBOT::PrintS(char* s)
{
	StringToAdd.Format(s);
	MsgHandle->Add_Message(StringToAdd);
}

void ROBOT::PrintL(char* s)
{
	StringToAdd.Format(s);
	MsgHandle->Add_Message(StringToAdd);
	MsgHandle->Enter_Message();
}

void ROBOT::Print(CString s)
{
	MsgHandle->Add_Message(s);
	MsgHandle->Enter_Message();
}

void ROBOT::ENTER()
{
	MsgHandle->Enter_Message();
}

void ROBOT::TAB()
{
	PrintS("\t");
}

void ROBOT::Debug_Print(CString o)
{
	MsgHandle2->Add_Message(o);
	MsgHandle2->Enter_Message();
}

void ROBOT::SJ_DEBUG(int type, char* str1, char* str2, int int1, float flt1)
{
	return;
	switch(type)
	{
	case String:
		PrintS(str1);
		break;
	case StringInt:
		PrintS(str1);
		PrintS(": ");
		PrintI(int1);
		break;
	case StringFloat:
		PrintS(str1);
		PrintS(": ");
		PrintF(flt1,1);
		break;
	case StringIntString:
		PrintS(str1);
		break;
	case StringString:
		PrintS(str1);
		PrintS(str2);
		break;
	case StringOnly:
		PrintS(str1);
		break;
	case StringIntOnly:
		PrintS(str1);
		PrintS(": ");
		PrintI(int1);
		break;
	case StringFloatOnly:
		PrintS(str1);
		PrintS(": ");
		PrintF(flt1,1);
		break;
	case StringIntStringOnly:
		PrintS(str1);
		break;
	case StringStringOnly:
		PrintS(str1);
		PrintS(str2);
		break;
	}
	if(Add_XYTheta
		&& type != StringOnly
		&& type != StringIntOnly
		&& type != StringFloatOnly
		&& type != StringIntStringOnly
		&& type != StringStringOnly
		)
	{
		PrintS("\t");PrintS("\t");PrintF(X,1);PrintS("\t");PrintF(Y,1);PrintS("\t");PrintF(Theta*R2D,1);
	}

	//PrintS("\r\n");
	ENTER();
}

